#include <hidef.h> /* for EnableInterrupts macro */
#include "platform.h" /* include peripheral declarations */
#include "io.h"

#include "sci.h"
#include "string.h"
#include "term.h"
#include "i2c.h"
#include "SSD1307.h"
#include "GDisp.h"
#include "GFONT.h"
#include "FDisp.h"
#include "Cour14.h"

volatile char result;


bool getIntState(void)
{
  #pragma MESSAGE DISABLE C1404        /* Suppress warning C1404: Return expected */
  asm 
  {
    CLRA
    BIL low
    COMA
    low:
  };
}


void hwTest(void)
{
  uint8 result, error = 0;
  termWrite("MC-CAR Hardware test...");
  
  result = i2cTest(0x1C << 1); // Accelerometer
  if (result != EC_SUCCESS)
  {
    termWriteLine("ERROR Accelerometer!");
    error++;
  }
 
    
  result = i2cTest(0x54); // Encoder
  if (result != EC_SUCCESS)
  {
    termWriteLine("ERROR Encoder!");
    error++;
  }
  
  result = i2cTest(0x39 << 1); // Color Sensor
  if (result != EC_SUCCESS)
  {
    termWriteLine("ERROR Color sensor!");
    error++;
  }
    
  result = i2cTest(0x78); // OLED Display
  if (result != EC_SUCCESS)
  {
    termWriteLine("ERROR OLED-Display!");
    error++;
  }
  
  if (error == 0) termWriteLine("No errors :-)");
}


void main(void) 
{
  signed char x, y;
  char string[]="MC-CAR";
  char string2[]="Name Vorname";
  //byte *str;
  EnableInterrupts;

  ioInit();
  sciInit(9600);
  i2cInit();
  sciWriteLine("mc bot ready :-)");
  ioSetLed(ledRear);
  
  hwTest();
  
  // configure accelerometer interrupt pin to open drain!
  result = i2cWriteCmdByte((0x1C << 1), 0x2C, 0x01); 
  
 
  // Stromversorgung OLED-Display einschalten
  result = i2cWriteCmdByte(0x54, 0x00, 0x20);
  
  // Display initialisieren
  SSD1307_Init();
  SSD1307_Clear();
  
/*
  // OLED Display Ansteuern
    result = displayOff();
    result = displayOn();
    result = initDisplay();
    result = setEntireDisplay();
    result = setContrast((unsigned char)0x00);   
    result = setAdressingMode(HORIZONTAL_MODE);    
    result = setColumns(0xAA);
    result = setColumns(0xAA);
    result = setColumns(0x00);
    result = setColumns(0xFF);
	result = setColumns(0xC7);
    result = setDisplay();
    result = setContrast((unsigned char)0xFF); 
*/

//  /*Alle Ecken setzen */
//  GDisp1_SetPixel(0,0);
//  SSD1307_UpdateFull();
//  GDisp1_SetPixel(95,0);
//  SSD1307_UpdateFull();
//  GDisp1_SetPixel(0,31);
//  SSD1307_UpdateFull();
//  GDisp1_SetPixel(95,31);
//  SSD1307_UpdateFull();
//  x=0;
//  y=0;
  
  /* Texte ausgeben */
//  str = &string;
//  FDisp1_WriteChar('H', &x, &y, GFONT1_GetFont());
//  SSD1307_UpdateFull();
  
  
//  FDisp1_WriteString(string,&x,&y,GFONT1_GetFont());
//  SSD1307_UpdateFull();
//  x=0;
//  y=8;
//  FDisp1_WriteString(string2,&x,&y,GFONT1_GetFont());
//  SSD1307_UpdateFull();
//  x=0;
//  y=16;
//  FDisp1_WriteString(string,&x,&y,GFONT1_GetFont());
//  SSD1307_UpdateFull();
//  x=0;
//  y=24;
//  FDisp1_WriteString(string2,&x,&y,GFONT1_GetFont());
//  SSD1307_UpdateFull();
  
  	
//    /* Beispielanwendung 1  */
//    x=0;
//    y=0;
//  	GDisp1_DrawBox(0,0,33,14,1);
//  	GDisp1_DrawBox(33,0,33,14,1);
//  	GDisp1_DrawBox(0,18,33,14,1);
//  	GDisp1_DrawBox(33,18,33,14,1);
//  	SSD1307_UpdateFull();
//  	GDisp1_DrawCircle(4,6,3);
//  	GDisp1_DrawFilledBox(11,3,3,7);
//  	//GDisp1_DrawCircle(12,6,3);
//  	GDisp1_DrawCircle(20,6,3);
//  	GDisp1_DrawCircle(20,25,3);
//  	GDisp1_DrawFilledBox(26,3,3,7);
//  	//GDisp1_DrawCircle(28,6,3);
//  	SSD1307_UpdateFull();
//  	x=70;
//  	y=3;
//  	FDisp1_WriteString("0x53",&x, &y, GFONT1_GetFont());
//  	SSD1307_UpdateFull();
  
//  	SSD1307_Clear();
  	
//   	/* Beispielanwendung 2 */
//    x=0;
//    y=0;
//  	FDisp1_WriteString(string, &x, &y, Cour14_GetFont());
//  	SSD1307_UpdateFull();
//  	x=0;
//  	y=22;
//  	FDisp1_WriteString(string2,&x,&y,GFONT1_GetFont());
//  	SSD1307_UpdateFull();
//  	x=85;
//  	y=15;
//  	FDisp1_WriteChar(1, &x, &y, Cour14_GetFont());
//  	SSD1307_UpdateFull();
//  	

  	
//  /* Formen zeichnen */
//  //void GDisp1_ClrPixel(GDisp1_PixelDim x, GDisp1_PixelDim y);
//  GDisp1_DrawFilledBox(30,2,30,8);
//  SSD1307_UpdateFull();
//  GDisp1_DrawBox(40,20,15,12,3);
//  SSD1307_UpdateFull();
//  GDisp1_DrawHLine(1,1,10);
//  SSD1307_UpdateFull();
//  GDisp1_DrawVLine(1,1,10);
//  SSD1307_UpdateFull();
//  GDisp1_DrawLine(1,1,9,9);
//  SSD1307_UpdateFull();
//  GDisp1_DrawCircle(15,15,8);
//  SSD1307_UpdateFull();
//  GDisp1_DrawFilledCircle(80,10,6);
//  SSD1307_UpdateFull();
  
  
   // Stromversorgung OLED-Dispilay ausschalten
   //result = i2cWriteCmdByte(0x54, 0x00, 0x00);

 
  
  for(;;) 
  {
    __RESET_WATCHDOG();	/* feeds the dog */
  }
}
